#include <ros/ros.h>
#include <gazebo_msgs/ModelStates.h>
#include <geometry_msgs/Pose.h>
#include <nav_msgs/Odometry.h>

#define NODE_NAME  "pub_gazebo_pos2control"

nav_msgs::Odometry odom;
gazebo_msgs::ModelStates model_states;
geometry_msgs::Pose pose;

bool received_model_state;

int uav_id = 1;
std::string uav_name("/uav" + std::to_string(uav_id));

void modelStateCb(const gazebo_msgs::ModelStates::ConstPtr &msg)
{
    model_states = *msg;
    int index;
    std::string _name;
    for(int i = 0; i < model_states.name.size(); i++)
    {
        if(model_states.name.at(i) == "iris" + std::to_string(uav_id))
        {
            if(!received_model_state) received_model_state = true;
            pose = model_states.pose.at(i);
            odom.header.stamp = ros::Time::now();
            odom.pose.pose = pose; 
        }
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, NODE_NAME);
    ros::NodeHandle nh;
    ros::Rate rate(20);
    ros::Subscriber model_state_sub = nh.subscribe<gazebo_msgs::ModelStates>("/gazebo/model_states", 10, modelStateCb);
    ros::Publisher gazebo_pos_pub = nh.advertise<nav_msgs::Odometry>(uav_name + "/prometheus/ground_truth", 5);

    while(ros::ok())
    {
        ros::spinOnce();
        if(received_model_state) gazebo_pos_pub.publish(odom);
        rate.sleep();
    }
    return 0;
}   